﻿// 自由网平差
using MathNet.Numerics.LinearAlgebra;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Text;
using static AdjPlane.Angle_t;

namespace AdjPlane
{
    using poi_dic = Dictionary<string, Point2D>;
    using mat = Matrix<double>;
    using obs_dic = Dictionary<string,double>;
    using idx_dic = Dictionary<string,int>;
    
    //自由网平差
    public class FreeNet : Plane
    {
        //参数个数
        int u;

        //自由网选择两个相距最远的已知点计算近似坐标
        void ChooseKp(out poi_dic K)
        {
            K = new poi_dic();
            string st1 = "";
            string st2 = "";
            double max = 0;
            foreach (var p1 in knownPoint)
            {
                foreach (var p2 in knownPoint)
                {
                    if (p1.Key == p2.Key)
                        continue;
                    double x1 = p1.Value.X;
                    double y1 = p1.Value.Y;
                    double x2 = p2.Value.X;
                    double y2 = p2.Value.Y;
                    double dx = x2 - x1;
                    double dy = y2 - y1;
                    double s2 = dx * dx + dy * dy;
                    if (s2 > max)
                    {
                        max = s2;
                        st1 = p1.Key;
                        st2 = p2.Key;
                    }
                }
            }
            K.Add(st1, knownPoint[st1]);
            K.Add(st2, knownPoint[st2]);
        }
        void ChooseKp2(out poi_dic K)
        {
            K = new poi_dic();
            foreach (KeyValuePair<string, Point2D> item in knownPoint)
            {
                if (K.Count < 2)
                    K.Add(item.Key, item.Value);
            }
        }
        //方向误差方程
        new void GetFX1(obs_dic DXJ,ref mat A1, ref mat L1)
        {
            //all观测点计数
            int i = 0;
            //定向角计数
            int s = 0;
            //测站循环,计算误差方程矩阵
            foreach (KeyValuePair<string, obs_dic> sta in obsL)
            {//前obs_num行表示距离方程，后obs_num行表示方向方程；
                foreach (KeyValuePair<string, double> obs in sta.Value)//照准点循环
                {//前2*ukp_num列表示未知点参数，后CZ.Count列表示定向角未知数
                    double dx = Result[obs.Key].X - Result[sta.Key].X;
                    double dy = Result[obs.Key].Y - Result[sta.Key].Y;
                    double azi = GetAzi(dx, dy);
                    double S = Math.Sqrt(dx * dx + dy * dy);
                    double a = rou * Math.Sin(azi) / S * 0.001;
                    double b = rou * Math.Cos(azi) / S * 0.001;
                    int j = pointIndex[sta.Key]; //测站点的编号
                    int k = pointIndex[obs.Key];//照准点的编号
                    //方向方程系数阵（"/mm)
                    A1[i, 2 * j] = a;
                    A1[i, 2 * j + 1] = -b;
                    A1[i, 2 * k] = -a;
                    A1[i, 2 * k + 1] = b;
                    //定向角未知数对应的列系数，放在所有位置参数（XY）之后  
                    A1[i, 2 * p_num + s] = -1.0;
                    //方位角观测值
                    double L = Keep_0To360(DMS2D(obs.Value) + DXJ[sta.Key]);
                    double L0 = azi * 180 / Math.PI;
                    L1[i, 0] = (L - azi * 180 / Math.PI) * 3600;
                    i++;//进入下一个照准点（不是观测值）
                }
                s++;//进入下一个测站
            }
        }
        //距离误差方程
        new void GetJL1(ref mat A2, ref mat L2)
        {
            int i = 0; //距离观测值计数
            int s = 0;//测站计数 
            foreach (KeyValuePair<string, obs_dic> sta in obsS)//测站循环,计算误差方程矩阵
            {//前obs_num行表示距离方程，后obs_num行表示方向方程；
                foreach (KeyValuePair<string, double> obs in sta.Value)//照准点循环
                {//前2*ukp_num列表示未知点参数，后CZ.Count列表示定向角未知数
                    double dx = Result[obs.Key].X - Result[sta.Key].X;
                    double dy = Result[obs.Key].Y - Result[sta.Key].Y;
                    double S = Math.Sqrt(dx * dx + dy * dy);
                    double c = dx / S;
                    double d = dy / S;
                    int j = pointIndex[sta.Key]; //测站点的编号
                    int k = pointIndex[obs.Key];//照准点的编号
                    A2[i, 2 * j] = -1.0 * c;//距离方程系数阵(1)
                    A2[i, 2 * j + 1] = -1.0 * d;
                    A2[i, 2 * k] = c;
                    A2[i, 2 * k + 1] = d;
                    L2[i, 0] = 1000 * (obs.Value - S);//距离方程常数项(mm)（=近似距离-观测距离）
                    i++;//进入下一个观测点（不是观测值）
                }
                s++;//进入下一个测站
            }
        }
        //自由网N求逆
        //ch选择求逆法，isget_qx是否计算QX
        // ref [5],[6],[7] ([5],[6]论文似乎有些问题)
        void GetQp(int ch, ref mat Qp, int isget_qx, ref mat QX)
        {
            if (ch == 1)
            {
                //-----1附加条件法
                int isnorm = 0;
                GetS(Result, pointIndex, CZ, out mat S1, out mat S2, isnorm);
                var S = S1;
                var SS = S.Transpose() * S;
                var Px = mat.Build.DiagonalIdentity(u, u);
                var PxS = Px * S;
                Qp = (N + PxS * PxS.Transpose()).Inverse();
                if (isget_qx == 1)
                {
                    if (isnorm == 1)
                        QX = Qp - S * S.Transpose();
                    else
                        QX = Qp - Qp * PxS * PxS.Transpose() * Qp;
                }
            }
            else if (ch == 2)
            {
                //----2附加约束+重心化+标准化
                int iscenter = 1;
                int isnorm = 0;
                GetG(Result, pointIndex, CZ, out mat G1, out mat G2, iscenter, isnorm);
                var G = G1;
                var GG = G.Transpose() * G;
                var Px = mat.Build.DiagonalIdentity(u, u);
                var PxG = Px * G;
                Qp = (N + PxG * PxG.Transpose()).Inverse();
                if (isget_qx == 1)
                {
                    if (isnorm == 1)
                        QX = Qp - G * G.Transpose();
                    else
                        QX = Qp - Qp * PxG * PxG.Transpose() * Qp;
                }
            }
            else if (ch == 3)
            {
                //-----3伪逆法
                Qp = N.PseudoInverse();
                if (isget_qx == 1)
                    QX = Qp * N * Qp.Transpose();
            }
        }
        //----2附加约束+重心化+标准化
        //is_center选择是否重心化，is_norm选择是否标准化
        void GetG(poi_dic Result, idx_dic pointIndex,
            List<string> CZ, out mat G1, out mat G2, int is_center, int is_norm)
        {
            int d = 4;
            mat GT1 = mat.Build.Dense(d, u, 0);
            mat GT2 = mat.Build.Dense(d, u, 0);
            double sumx = 0, sumy = 0;
            foreach (KeyValuePair<string, Point2D> item in Result)
            {
                double x = item.Value.X;
                double y = item.Value.Y;
                sumx += x;
                sumy += y;
            }
            int num = Result.Count;
            double ave_x = 0, ave_y = 0;
            if (is_center == 1)
            {
                ave_x = sumx / num;
                ave_y = sumy / num;
            }
            double H = 0;
            foreach (KeyValuePair<string, int> item in pointIndex)
            {
                string s = item.Key;
                double dx = Result[s].X - ave_x;
                double dy = Result[s].Y - ave_y;
                H += dx * dx + dy * dy;
            }
            double H1 = H + CZ.Count * rou * rou;
            double sqrtu = 1, sqrtH = 1, sqrtH1 = 1;
            if (is_norm == 1)
            {
                sqrtu = Math.Sqrt(num);
                sqrtH = Math.Sqrt(H);
                sqrtH1 = Math.Sqrt(H1);
            }
            foreach (KeyValuePair<string, int> item in pointIndex)
            {
                string s = item.Key;
                double dx = Result[s].X - ave_x;
                double dy = Result[s].Y - ave_y;
                int j = item.Value;

                GT1[0, 2 * j] = 1 / sqrtu;
                GT1[1, 2 * j + 1] = 1 / sqrtu;
                GT1[2, 2 * j] = -dy / sqrtH1;
                GT1[2, 2 * j + 1] = dx / sqrtH1;
                GT1[3, 2 * j] = dx / sqrtH;
                GT1[3, 2 * j + 1] = dy / sqrtH;

                GT2[0, 2 * j] = 1 / sqrtu;
                GT2[1, 2 * j + 1] = 1 / sqrtu;
                GT2[2, 2 * j] = -dy / sqrtH;
                GT2[2, 2 * j + 1] = dx / sqrtH;
                GT2[3, 2 * j] = dx / sqrtH;
                GT2[3, 2 * j + 1] = dy / sqrtH;
            }
            for (int i = 0; i < CZ.Count; i++)
            {
                GT1[2, i + 2 * pointIndex.Count] = rou / sqrtH1;
            }
            //GT1考虑定向角参数，GT2不考虑定向角参数
            //当网中有长度基准时，去掉第四行
            GT1 = GT1.SubMatrix(0, 3, 0, GT1.ColumnCount);
            GT2 = GT2.SubMatrix(0, 3, 0, GT2.ColumnCount);
            //==================
            G1 = GT1.Transpose();
            G2 = GT2.Transpose();
        }
        //附加条件矩阵，S1考虑定向角参数，S2不考虑,is_norm 选择是否标准化
        void GetS(poi_dic Result, idx_dic pointIndex,
            List<string> CZ, out mat S1, out mat S2, int is_norm)
        {
            int d = 4;
            var ST1 = Newmat(d, u);
            var ST2 = Newmat(d, u);
            int num = Result.Count;

            double H = 0;
            foreach (KeyValuePair<string, Point2D> item in Result)
            {
                double x = item.Value.X;
                double y = item.Value.Y;
                H += x * x + y * y;
            }
            double H1 = H + CZ.Count * rou * rou;
            double sqrtu = 1, sqrtH = 1, sqrtH1 = 1;
            if (is_norm == 1)
            {
                sqrtu = Math.Sqrt(num);
                sqrtH = Math.Sqrt(H);
                sqrtH1 = Math.Sqrt(H1);
            }
            foreach (KeyValuePair<string, int> item in pointIndex)
            {
                string s = item.Key;
                double x0 = Result[s].X;
                double y0 = Result[s].Y;
                int j = item.Value;
                ST1[0, 2 * j] = 1 / sqrtu;
                ST1[1, 2 * j + 1] = 1 / sqrtu;
                ST1[2, 2 * j] = -y0 / sqrtH1;
                ST1[2, 2 * j + 1] = x0 / sqrtH1;
                ST1[3, 2 * j] = x0 / sqrtH1;
                ST1[3, 2 * j + 1] = y0 / sqrtH1;

                ST2[0, 2 * j] = 1;
                ST2[1, 2 * j + 1] = 1;
                ST2[2, 2 * j] = -1.0 * y0;
                ST2[2, 2 * j + 1] = x0;
                ST2[3, 2 * j] = x0;
                ST2[3, 2 * j + 1] = y0;
            }
            for (int i = 0; i < CZ.Count; i++)
            {
                ST1[2, i + 2 * pointIndex.Count] = rou / sqrtH1;
            }
            //如果有长度基准，删除第四行
            ST1 = ST1.SubMatrix(0, 3, 0, ST1.ColumnCount);
            ST2 = ST2.SubMatrix(0, 3, 0, ST2.ColumnCount);
            S1 = ST1.Transpose();
            S2 = ST2.Transpose();
        }
        //坐标协因数转换矩阵
        mat GetK(double theta0, int u)
        {
            mat K = Newmat(u, u);
            for (int i = 0; i < u-1;)
            {
                double cosa = Math.Cos(theta0);
                double sina = Math.Sin(theta0);
                K[i, i] = cosa;
                K[i, i + 1] = sina;
                K[i + 1, i] = -sina;
                K[i + 1, i + 1] = cosa;
                i = i + 2;
            }
            return K;
        }
        //基准权
        mat GetPx(int u)
        {
            mat Px1 = mat.Build.Dense(3, 3, 0);
            mat Px2 = mat.Build.Dense(u - 3, u - 3, 1);
            return Px1.DiagonalStack(Px2);
        }
        /// <summary>
        /// 三参数转换（置平）
        /// </summary>
        /// <param name="prev">目标坐标系</param>
        /// <param name="next">待转换的坐标</param>
        /// <param name="C">转换参数 x,y,theta</param>
        /// <param name="iter">是否迭代,1是0否</param>
        /// <returns>转换后的坐标</returns>
        poi_dic CoordinateTrans3(poi_dic prev, poi_dic next, out double[] C, int iter)
        {
            //SortedList<T>可以对键的集合进行索引，Dictionary不行
            //V=B*X-L；VTPV=min;==>X=(BTPB)'*(BTPL)

            poi_dic result = new poi_dic();//存储转换后测站内的点
            poi_dic Sprev = new poi_dic();//重合点在上个坐标系的坐标
            poi_dic Snext = new poi_dic();//重合点在下个坐标系的坐标
            foreach (KeyValuePair<string, Point2D> item in prev)
            {
                if (next.ContainsKey(item.Key))
                    Sprev.Add(item.Key, item.Value);
            }
            foreach (KeyValuePair<string, Point2D> item in next)
            {
                if (Sprev.ContainsKey(item.Key))
                    Snext.Add(item.Key, item.Value);
            }
            var A = mat.Build.Dense(2 * Snext.Count, 3, 0);//平面坐标转换系数阵
            var L = mat.Build.Dense(2 * Snext.Count, 1, 0);//观测值(常数项）
            var X = mat.Build.Dense(3, 1, 0);//参数迭代值，初值设为零

            double delta_x0 = 0;//平移因子初值
            double delta_y0 = 0;
            double theta0 = 0;//旋转因子初值
            double cosa = 0;
            double sina = 0;
            int count = 0;//迭代次数
            mat X_prev;
            while (true)
            {
                int i = 0;
                foreach (KeyValuePair<string, Point2D> kv in Snext)
                { 
                    double x0 = kv.Value.X;
                    double y0 = kv.Value.Y;
                    cosa = Math.Cos(theta0);
                    sina = Math.Sin(theta0);
                    double a1 = -x0 * sina + y0 * cosa;
                    double a2 = x0 * cosa + y0 * sina;
                    double a3 = -a2;
                    double a4 = a1;

                    A[2 * i, 0] = 1;
                    A[2 * i, 1] = 0;
                    A[2 * i, 2] = a1;
                    A[2 * i + 1, 0] = 0;
                    A[2 * i + 1, 1] = 1;
                    A[2 * i + 1, 2] = a3;

                    double x1 = Sprev[kv.Key].X;
                    double y1 = Sprev[kv.Key].Y;
                    L[2 * i, 0] = x1 - delta_x0 - a2;
                    L[2 * i + 1, 0] = y1 - delta_y0 - a4;
                    i++;
                }
                var AT = A.Transpose();
                var x = ((AT * A).Inverse() * AT * L);
                X_prev = X;
                X += x;//参数值迭代
                var dX = X - X_prev;
                double eps = 0;
                for (int j = 0; j < 3; j++)
                {
                    double dx = Math.Abs(dX[j, 0]);
                    if (eps < dx)
                        eps = dx;
                }
                count++;
                delta_x0 = X[0, 0];
                delta_y0 = X[1, 0];
                theta0 = X[2, 0];
                cosa = Math.Cos(theta0);
                sina = Math.Sin(theta0);
                if (eps < 1e-5 || iter == 0)
                    break;
            }

            double[,] n1 = { { delta_x0 }, { delta_y0 } };//平移
            double[,] n2 = { { cosa, sina }, { -sina, cosa } };//旋转
            var N1 = mat.Build.DenseOfArray(n1);
            var N2 = mat.Build.DenseOfArray(n2);
            //把第二测站的坐标全转换到第一个测站的坐标系中
            foreach (KeyValuePair<string, Point2D> kv in next)
            {
                double[,] n3 = { { kv.Value.X }, { kv.Value.Y } };
                var N3 = mat.Build.DenseOfArray(n3);
                var N4 = N1 + N2 * N3; //转换后的坐标
                if (!result.ContainsKey(kv.Key))
                    result.Add(kv.Key, new Point2D(N4[0, 0], N4[1, 0]));
            }
            C = new double[3];
            C[0] = delta_x0;
            C[1] = delta_y0;
            C[2] = theta0;
            return result;
        }

        //输出三参数转换结果（置平）
        void Output_3p(string outpath, poi_dic knownPoint, poi_dic Result, idx_dic pointIndex)
        {
            var Result3 = CoordinateTrans3(knownPoint, Result, out double[] C, 1);
            var G = GetK(C[2], u);
            var QX3 = G * QX * G.Transpose();
            double theta = RD2DMS(C[2]);
            List<string> str = new List<string>();
            str.Add(string.Format("{0,30}", "三参数坐标转换"));
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "转换参数"));
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}{1,20}", "旋转参数/dms:", theta.ToString("f6")));
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "三参数坐标转换后坐标及其精度"));
            str.Add("---------------------------------------------------------------------");
            OutputP(ref str, pointIndex, Result3, QX3);
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "已知点坐标与三参数转换坐标比较"));
            str.Add("---------------------------------------------------------------------");
            OutputKPTP(ref str, knownPoint, Result3);
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "已知点间坐标差之差及相对精度"));
            str.Add("---------------------------------------------------------------------");
            OutputKPKP(ref str, knownPoint, Result3);
            str.Add("---------------------------------------------------------------------");
            File.AppendAllLines(outpath, str);
        }
        //四参数转换结果输出
        void Output_4p(string outpath, poi_dic knownPoint, poi_dic Result, idx_dic pointIndex)
        {
            //var Result3 = CoordinateTrans3(knownPoint, Result, out double[] D);
            var Result4 = CoordinateTrans4(knownPoint, Result, out double[] C);
            var G = GetK(C[3], u);
            var QX4 = G * QX * G.Transpose();
            double theta = RD2DMS(C[3]);
            double k = C[2];
            List<string> str = new List<string>();
            str.Add(string.Format("{0,30}", "四参数坐标转换"));
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "转换参数"));
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}{1,20}", "旋转参数/dms:", theta.ToString("f6")));
            str.Add(string.Format("{0,30}{1,20}", "尺度参数/ppm:", (k * 1E+6).ToString("f6")));
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "四参数坐标转换后坐标及其精度"));
            str.Add("---------------------------------------------------------------------");
            OutputP(ref str, pointIndex, Result4, QX4);
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "已知点坐标与四参数转换坐标比较"));
            str.Add("---------------------------------------------------------------------");
            OutputKPTP(ref str, knownPoint, Result4);
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "已知点间坐标差之差及相对精度"));
            str.Add("---------------------------------------------------------------------");
            OutputKPKP(ref str, knownPoint, Result4);
            str.Add("---------------------------------------------------------------------");
            File.AppendAllLines(outpath, str);
        }
        //点位信息
        void OutputP(ref List<string> str, idx_dic pIndex, poi_dic Result, mat QX)
        {
            str.Add(string.Format("{0,13}{1,16}{2,15}{3,8}{4,8}{5,8}{6,8}{7,8}{8,11}",
                "Name", "X/m", "Y/m", "MX/mm", "MY/mm", "MP/mm", "E/mm", "F/mm", "T/dms"));
            foreach (KeyValuePair<string, int> p in pIndex)
            {
                double X = Result[p.Key].X;
                double Y = Result[p.Key].Y;
                //*****************计算坐标协因数******************
                double Qxx = QX[2 * p.Value, 2 * p.Value];
                double Qyy = QX[2 * p.Value + 1, 2 * p.Value + 1];
                double Qxy = QX[2 * p.Value, 2 * p.Value + 1];
                double MX = sigma * Math.Sqrt(Qxx);
                double MY = sigma * Math.Sqrt(Qyy);
                double MP = Math.Sqrt(MX * MX + MY * MY);
                //*****************计算误差椭圆******************
                double K = Math.Sqrt(Math.Pow(Qxx - Qyy, 2) + 4 * Qxy * Qxy);
                double E = sigma * Math.Sqrt(0.5 * (Qxx + Qyy + K));
                double F = sigma * Math.Sqrt(0.5 * (Qxx + Qyy - K));
                //*************误差椭圆极大方向①*******
                double Qe = 0.5 * (Qxx + Qyy + K);
                double Te = Keep_0To360(RD2D(Math.Atan((Qe - Qxx) / Qxy)));
                if (Te >= 180) //极大方向是反方向的两个，取较小的
                    Te -= 180;
                double T = D2DMS(Te);
                str.Add(string.Format("{0,13}{1,16}{2,15}{3,8}{4,8}{5,8}{6,8}{7,8}{8,11}", p.Key,
                    X.ToString("f5"), Y.ToString("f5"), MX.ToString("f2"), MY.ToString("f2"),
                    MP.ToString("f2"), E.ToString("f2"), F.ToString("f2"), T.ToString("f4")));
            }
        }
        //已知坐标与转换坐标比较
        void OutputKPTP(ref List<string> str, poi_dic knownPoint, poi_dic Result)
        {
            str.Add(string.Format("{0,13}{1,20}{2,30}{3,25}", "Name", "已知坐标X,Y/m",
                "转换坐标X,Y/m", "较差DX,DY/mm"));
            foreach (KeyValuePair<string, Point2D> item in knownPoint)
            {
                string name = item.Key;
                double Xk = item.Value.X;
                double Yk = item.Value.Y;
                double Xt = Result[name].X;
                double Yt = Result[name].Y;
                double dx = (Xk - Xt) * 1000;
                double dy = (Yk - Yt) * 1000;
                str.Add(string.Format("{0,13}{1,15}{2,15}{3,20}{4,15}{5,10}{6,10}",
                    name, Xk.ToString("f5"), Yk.ToString("f5"), Xt.ToString("f5"),
                    Yt.ToString("f5"), dx.ToString("f2"), dy.ToString("f2")));
            }
        }
        //已知点间坐标差之差及相对精度
        void OutputKPKP(ref List<string> str, poi_dic knownPoint, poi_dic Result)
        {
            str.Add(string.Format("{0,13}{1,13}{2,20}{3,20}{4,12}{5,7}", "From", "To",
                "已知坐标增量DX,DY/m", "转换坐标增量DX,DY/m", "坐标差之差/mm", "相对精度"));
            int i = 0;
            foreach (KeyValuePair<string, Point2D> Sta in knownPoint)
            {
                string sname = Sta.Key;
                double Xk1 = Sta.Value.X;
                double Yk1 = Sta.Value.Y;
                double Xt1 = Result[sname].X;
                double Yt1 = Result[sname].Y;
                int j = 0;
                foreach (KeyValuePair<string, Point2D> End in knownPoint)
                {
                    if (i >= j)
                    {
                        j++;
                        continue;
                    }
                    string ename = End.Key;
                    double Xk2 = End.Value.X;
                    double Yk2 = End.Value.Y;
                    double Xt2 = Result[ename].X;
                    double Yt2 = Result[ename].Y;

                    double dXk = Xk2 - Xk1;
                    double dYk = Yk2 - Yk1;
                    double dXt = Xt2 - Xt1;
                    double dYt = Yt2 - Yt1;

                    double ddx = dXk - dXt;
                    double ddy = dYk - dYt;

                    double ds = Math.Sqrt(ddx * ddx + ddy * ddy);
                    double S = Math.Sqrt(dXk * dXk + dYk * dYk);
                    double S_ds = S / ds;

                    str.Add(string.Format("{0,13}{1,13}{2,13}{3,13}{4,13}{5,13}{6,8}{7,8}{8,10}",
                    sname, ename, dXk.ToString("f5"), dYk.ToString("f5"), dXt.ToString("f5"),
                    dYt.ToString("f5"), (ddx * 1000).ToString("f2"), (ddy * 1000).ToString("f2"),
                    S_ds.ToString("f0")));
                    j++;
                }
                i++;
            }
        }
        //输出点位信息
        new void OutputXY(string outpath, idx_dic pointIndex)
        {
            List<string> str = new List<string>();
            str.Add(string.Format("{0,30}", "平差坐标及其精度"));
            str.Add("---------------------------------------------------------------------");
            string weakP = string.Empty;
            double weakX = 0, weakY = 0, weakMX = 0, weakMY = 0, weakMP = 0,
                weakE = 0, weakF = 0, weakT = 0;
            double MxSum = 0, MySum = 0, MpSum = 0;
            str.Add(string.Format("{0,13}{1,16}{2,15}{3,8}{4,8}{5,8}{6,8}{7,8}{8,11}",
                 "Name", "X/m", "Y/m", "MX/mm", "MY/mm", "MP/mm", "E/mm", "F/mm", "T/dms"));
            foreach (KeyValuePair<string, int> p in pointIndex)
            {//未知点
                double Te = 0, T = 0;
                double X = Result[p.Key].X;
                double Y = Result[p.Key].Y;
                //*****************计算坐标协因数******************
                double Qxx = QX[2 * p.Value, 2 * p.Value];
                double Qyy = QX[2 * p.Value + 1, 2 * p.Value + 1];
                double Qxy = QX[2 * p.Value, 2 * p.Value + 1];
                double MX = sigma * Math.Sqrt(Qxx);
                double MY = sigma * Math.Sqrt(Qyy);
                double MP = Math.Sqrt(MX * MX + MY * MY);
                MxSum += MX; MySum += MY; MpSum += MP;
                //*****************计算误差椭圆******************
                double K = Math.Sqrt(Math.Pow(Qxx - Qyy, 2) + 4 * Qxy * Qxy);
                double E = sigma * Math.Sqrt(0.5 * (Qxx + Qyy + K));
                double F = sigma * Math.Sqrt(0.5 * (Qxx + Qyy - K));
                //*************误差椭圆极大方向①*******
                double Qe = 0.5 * (Qxx + Qyy + K);
                Te = Keep_0To360(RD2D(Math.Atan((Qe - Qxx) / Qxy)));
                if (Te >= 180) //极大方向是反方向的两个，取较小的
                    Te -= 180;
                T = D2DMS(Te);
                if (weakMP < MP)
                { //寻找最弱点
                    weakMP = MP; weakMX = MX; weakMY = MY; weakP = p.Key;
                    weakX = X; weakY = Y; weakE = E; weakF = F; weakT = T;
                }
                str.Add(string.Format("{0,13}{1,16}{2,15}{3,8}{4,8}{5,8}{6,8}{7,8}{8,11}", p.Key,
                  X.ToString("f5"), Y.ToString("f5"), MX.ToString("f2"), MY.ToString("f2"),
                  MP.ToString("f2"), E.ToString("f2"), F.ToString("f2"), T.ToString("f4")));
            }
            str.Add("---------------------------------------------------------------------");
            string mx_ave = (MxSum / ukp_num).ToString("f2");
            string my_ave = (MySum / ukp_num).ToString("f2");
            string mp_ave = (MpSum / ukp_num).ToString("f2");
            str.Add(string.Format("{0,15}{1,-10}{2,15}{3,-10}{4,15}{5,-10}",
                "MX均值(mm):", mx_ave, "MY均值(mm):", my_ave, "MP均值(mm):", mp_ave));
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,30}", "最弱点及其精度"));
            str.Add("---------------------------------------------------------------------");
            str.Add(string.Format("{0,13}{1,16}{2,15}{3,8}{4,8}{5,8}{6,8}{7,8}{8,11}",
                "Name", "X/m", "Y/m", "MX/mm", "MY/mm", "MP/mm", "E/mm", "F/mm", "T/dms"));
            str.Add(string.Format("{0,13}{1,16}{2,15}{3,8}{4,8}{5,8}{6,8}{7,8}{8,11}", weakP,
                weakX.ToString("f5"), weakY.ToString("f5"), weakMX.ToString("f2"), weakMY.ToString("f2"),
                weakMP.ToString("f2"), weakE.ToString("f2"), weakF.ToString("f2"), weakT.ToString("f4")));
            str.Add("---------------------------------------------------------------------");
            str.Add("\n");
            File.AppendAllLines(outpath, str);
        }
        //输出文件标题
        new void OutputTitle(Proc_t popt,string outpath)
        {
            List<string> str = new List<string>();
            switch (popt.WgtDef)
            {
                case Weight.Tradition:
                    str.Add(string.Format("{0,40}", "平面自由网平差结果（常规定权）")); break;
                case Weight.hmt2:
                    str.Add(string.Format("{0,40}", "平面自由网平差结果（Helmert简化2,PVV/r）")); break;
            }
            str.Add("-----------------------------------------------------------------------------");
            File.WriteAllLines(outpath, str);
        }

        //所有结果输出
        new void Output(Proc_t popt, string outpath)
        {
            OutputTitle(popt,outpath);//标题
            OutputApprox(outpath, true);//概略坐标
            if (nl != 0)
                OutputL(outpath); //L
            if (ns != 0)
                OutputS(outpath);//S
            OutputXY(outpath, pointIndex);//点位
            OutputLS(outpath, pointIndex);//相对点位
            OutputProfile(popt,outpath);//概况
            if (popt.WgtDef!= Weight.Tradition && WgtDef < Weight.Robust)
                OutputVCE(outpath);//定权
            Output_3p(outpath, knownPoint, Result, pointIndex);
            Output_4p(outpath, knownPoint, Result, pointIndex);
        }
        //秩亏自由网平差
        new void Adjust(Proc_t popt)
        {
            //坐标迭代次数
            int iter = 0;
            u = 2 * pointIndex.Count + CZ.Count;//参数个数
            cov_res = new List<VCE_t>();
            iter_VCE = 1;//方差分量估计计数
            mat X_prev = Newmat(u, 1);
            Result = new poi_dic(Approx);//近似坐标作为迭代的初值
            //Flush_Matrix();//刷新变量

            //传参
            WgtDef = popt.WgtDef;//定权方法
            DisWgt = popt.DisWgt;//距离定权公式
            Eps_X = popt.Eps_X;//坐标迭代限差
            Eps_VCE = popt.Eps_VCE;//方差分量估计迭代限差
            IterMax_VCE = popt.IterMax_VCE;  //定权迭代最大次数

            M0 = alpha;
            //方差因子初值
            qv = M0 * M0;
            qvl = qv;
            qvs = qv;

            //定初权
            P1 = GetP1(nl, M0);
            P2 = GetP2(ns, M0, DisWgt);
            P = P1.DiagonalStack(P2);

            A1 = Newmat(nl, u);
            L1 = Newmat(nl, 1);
            A2 = Newmat(ns, u);
            L2 = Newmat(ns, 1);

            //建立误差方程矩阵
            //定向角：测站第一个照准方向的方位角
            Orientation_Angle(out obs_dic DXJ);//计算定向角
            GetFX1(DXJ, ref A1, ref L1);
            GetJL1(ref A2, ref L2);
            A = A1.Stack(A2);
            L = L1.Stack(L2);

            //法方程矩阵
            N1 = A1.Transpose() * P1 * A1;
            W1 = A1.Transpose() * P1 * L1;
            N2 = A2.Transpose() * P2 * A2;
            W2 = A2.Transpose() * P2 * L2;
            N = N1 + N2;
            W = W1 + W2;


            //=====计算法方程系数阵 求逆
            //----1附加约束法
            //----2附加约束+重心化+标准化
            //----3伪逆法
            int ch = 3;
            mat Qp;
            Qp = Newmat(u, u);
            QX = Newmat(u, u);
            GetQp(ch, ref Qp, 0, ref QX);
            X = Qp * W;

            V1 = A1 * X - L1;
            V2 = A2 * X - L2;

            if (WgtDef != Weight.Tradition)
            {
                V1 = A1 * X - L1;
                V2 = A2 * X - L2;
                PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
            }

            while (true)  //迭代近似坐标，误差方程变化
            {
                if (WgtDef == Weight.hmt2)
                    SimplfyHe2();

                //重新平差
                N1 = A1.Transpose() * P1 * A1;
                W1 = A1.Transpose() * P1 * L1;
                N2 = A2.Transpose() * P2 * A2;
                W2 = A2.Transpose() * P2 * L2;
                N = N1 + N2;
                W = W1 + W2;

                GetQp(ch, ref Qp, 0, ref QX);
                X = Qp * W;

                V1 = A1 * X - L1;
                V2 = A2 * X - L2;

                if (WgtDef != Weight.Tradition)
                {
                    V1 = A1 * X - L1;
                    V2 = A2 * X - L2;
                    PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                    PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
                }

                //获取坐标改正数列向量的最大值
                double eps = 0;//前后迭代的改正数之差的最大值
                var dX = X - X_prev;
                for (int j = 0; j < X.ColumnCount; j++)
                {
                    double error = Math.Abs(dX[j, 0]);
                    if (eps < error)
                        eps = error;  //获得最大残差
                }
                X_prev = X;

                //坐标迭代
                iter++;//迭代次数
                foreach (KeyValuePair<string, int> dic in pointIndex)
                {
                    Result[dic.Key].X += X[2 * dic.Value, 0] / 1000;
                    Result[dic.Key].Y += X[2 * dic.Value + 1, 0] / 1000;
                }

                //跳出外循环
                if (eps < Eps_X)  //判断是否结束近似坐标迭代
                {
                    //存储方差分量估计的定权结果(最终的）
                    VCE_t w = new VCE_t
                    {
                        count = iter_VCE,
                        ql = qvl,
                        qs = qvs
                    };
                    iter_VCE++;
                    cov_res.Add(w);
                    break;
                }

                //如果不收敛，避免死锁
                if (iter == popt.IterMax_X)
                {
                    break;
                }
                //如果没达到迭代阈值，重新建立误差方程；坐标迭代后，误差方程矩阵也跟着改变
                Orientation_Angle(out DXJ);
                GetFX1(DXJ, ref A1, ref L1);
                GetJL1(ref A2, ref L2);
            }
            //计算验后单位权中误差
            PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
            PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
            PVV = PVV1 + PVV2;
            r = n + 3 - u;
            sigma = Math.Sqrt(PVV / r);
            if (WgtDef == Weight.Tradition)
                fai = M0;
            else
                fai = sigma; //验后单位权中误差

            A = A1.Stack(A2);
            L = L1.Stack(L2);
            P = P1.DiagonalStack(P2);

            GetQp(ch, ref Qp, 1, ref QX);
            var I = mat.Build.DenseIdentity(n, n);
            mat ROC = I - A * QX * A.Transpose() * P;//多余观测分量Qvv*P

            //  存储信息
            int i = 0;
            Q1 = P1.Inverse(); Q2 = P2.Inverse();
            foreach (KeyValuePair<string, obs_dic> sta in obsL)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    int ids = FindStaName(sta.Key);
                    int ido = FindObsName(ids, obs.Key);
                    double v = (A1 * X)[i, 0] - L1[i, 0];
                    V1[i, 0] = v;
                    AllObs[ids].data[ido].vl = v;
                    AllObs[ids].data[ido].qL = Q1[i, i];
                    AllObs[ids].data[ido].rocL = ROC[i, i];
                    i++;
                }
            }
            i = 0;
            foreach (KeyValuePair<string, obs_dic> sta in obsS)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    int ids = FindStaName(sta.Key);
                    int ido = FindObsName(ids, obs.Key);
                    double v = (A2 * X)[i, 0] - L2[i, 0];
                    V2[i, 0] = v;
                    AllObs[ids].data[ido].vs = v;
                    AllObs[ids].data[ido].qS = Q2[i, i];
                    AllObs[ids].data[ido].rocS = ROC[nl + i, nl + i];
                    i++;
                }
            }

        }
        //自由网求近似坐标,从端点开始计算假设坐标
        new void ApproxFree()
        {
            var localData = Polar(AllObs);
            //由站心坐标转换得到的假设坐标
            List<poi_dic> Suppose_raw = new List<poi_dic>();
            //第一测站的数据
            poi_dic result = localData[0];
            poi_dic suppose;
            Suppose_raw.Add(result);
            int num = 0;
            while (true)
            {
                num++;
                for (int i = 1; i < localData.Count; i++)
                {
                    poi_dic next = localData[i];
                    int exist_ukp = 0;
                    foreach (KeyValuePair<string, Point2D> p in next)
                    {
                        if (!Approx.ContainsKey(p.Key))
                        {
                            exist_ukp = -1;//当前测站存在未知点
                            break;
                        }
                    }
                    if (exist_ukp != -1)
                        continue;
                    var target = Connect(next, Suppose_raw);//在现有的假设坐标中找与next关联的测站
                    if (target.Count != 0) //找到了
                    {
                        result = Trans(target, next);//next转换后的坐标
                        Suppose_raw.Add(result);
                    }
                }
                suppose = Remove(Suppose_raw);//剔除重复的点位,顺便转换格式
                if (suppose.Count == PointName.Count)
                    break;
                if (num > 10)
                    break;
            }
            Approx = suppose;//基于假设坐标系
            //poi_dic K;
            //ChooseKp(out K);
            //Approx = Trans(knownPoint, suppose);
            //Approx = CoordinateTrans3(knownPoint, suppose, out double[] C, 0);
        }

        // 自由网平差求概略坐标时，
        // 本程序将第一个测站设为(0,0)，第一个照准方向的方位角设为0；
        new void Approximate(Proc_t popt, ref File_t fopt)
        {
            ApproxFree();
            string staName0 = AllObs[0].name;
            string obsName0 = AllObs[0].data[0].name;
            if (!Approx.ContainsKey(staName0))
                Approx.Add(staName0, new Point2D());
            Azi_t azi = new Azi_t();
            azi.beg = staName0;
            azi.end = obsName0;
            azi.azi = 0;
            Azis.Add(azi);
            ApproxFix(popt, ref fopt, AllObs, PointName.Count);
            CopyApprox = CopyDic(Approx);
        }
        //结果文件命名
        new void Name(Proc_t param, ref File_t fopt)
        {
            string v1 = ".ous", v2 = ".out";
            string p = Path.GetFileNameWithoutExtension(fopt.infile);
            string name = string.Empty;
            switch (param.WgtDef)
            {
                case Weight.Tradition: name = p + v1; break;
                case Weight.hmt2: name = p + v2; break;
            }
            fopt.oufile = Path.Combine(Path.GetDirectoryName(fopt.infile), name);
        }
        //自由网平差
        public void FreeAdj(Proc_t popt, ref File_t fopt)
        {
            global_popt = popt;
            Name(popt, ref fopt);
            ReadFile(fopt.infile, popt);//读取
            Approximate(popt, ref fopt);
            if (Approx.Count < PointName.Count)
            {
                fopt.oufile = "";
                fopt.mapfile = "";
                return;
            }
            Adjust(popt);//平差
            Output(popt, fopt.oufile);
        }
    }
}
